import os
import sys
import yaml
import ament_index_python
from ament_index_python.packages import get_package_share_directory
sys.path.append(os.path.join(get_package_share_directory('rm_vision_bringup'), 'launch'))


def generate_launch_description():
    import launch_ros
    from common import launch_params, robot_state_publisher, node_params, tracker_node
    # from common import gimbal_link2camera_color_optical_frame
    from launch_ros.actions import Node
    from launch import LaunchDescription

    # send the launch mode to the detector_node

    robot_type_params = yaml.safe_load(open(os.path.join(
        get_package_share_directory('rm_vision_bringup'), 'config', 'robot_type.yaml')))

    my_runtime = robot_type_params["running_on"]
    our_color = robot_type_params["our_color"]
    print(robot_type_params["is_sentry"])
    is_sentry = robot_type_params["is_sentry"]
    print(is_sentry)
    detector_node = Node(
        package='armor_detector',
        executable='armor_detector_node',
        emulate_tty=True,
        output='both',
        parameters=[node_params, {'running_on': my_runtime}, {'our_color': our_color}, {'is_sentry': is_sentry}],
        # arguments=['--ros-args', '--log-level',
        #            'armor_detector:='+launch_params['detector_log_level']],
    )

    return LaunchDescription([
        robot_state_publisher,
        detector_node,
        tracker_node,
        # gimbal_link2camera_color_optical_frame,
    ])
